#include <gazebo.hh>
#include <ghor_gazebo_plugin/gazebo_ros_world.h>
 

using namespace gazebo;


GazeboRosWorld::GazeboRosWorld(){
       printf("GazeboRosWorld \n");
}

GazeboRosWorld::~GazeboRosWorld()
{
}


void GazeboRosWorld::Load(physics::WorldPtr _world, sdf::ElementPtr _sdf){

// _world->InsertModel("models/box.model");

     for( int i=0;i<100;i++){
       for(int j=0;j<100;j++){

         sdf::SDF* particle=createParticle(i,j);
          _world->InsertModel(*particle);


       }  

     } 


};


sdf::SDF* GazeboRosWorld::createParticle(int i,int j){

   sdf::SDF* particle=new sdf::SDF();

   char buffer[1000];

   sprintf(buffer, "<gazebo version ='1.0'>\
          <model name ='sphere'>\
            <origin pose ='2 4 0 0 0 0'/>\
            <link name ='link'>\
              <!--<origin pose ='0 0 .5 0 0 0'/>-->\
              <inertial mass ='1.0'>\
                  <inertia ixx ='1' ixy ='0' ixz ='0'\
                           iyy ='1' iyz ='0' izz ='1'/>\
              </inertial>\
              <collision name ='collision'>\
             <geometry>\
                  <sphere radius ='0.03'/>\
                </geometry>\
              </collision>\
              <visual name ='visual'>\
                <geometry>\
                  <sphere radius ='0.1'/>\
                </geometry>\
                <material script ='Gazebo/Grey'/>\
              </visual>\
            </link>\
          </model>\
        </gazebo>",i,j);


  // printf(buffer);


   particle->SetFromString(buffer);

   return particle;

}




GZ_REGISTER_WORLD_PLUGIN(GazeboRosWorld)


 


